#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,trajectory_name_;
ros::Publisher cloud_pub;
bool  is_get_first_frame_ =  false;
tf::Transform T_mas_last_,T_mas_cur_;

// void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
// {
//     //Create & write .bin file
// //    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
//     ofstream bin_file(out_file.c_str());
//     if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

//     //PCD 2 BIN
//     cout << "Converting "<<endl;
//     for (size_t i = 0; i < cloud->points.size (); ++i)
//     {
//         bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
//         bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
//         //cout<< 	cloud->points[i]<<endl;
//     }

//     bin_file.close();
// }

// void saveCalib(string& out_file)
// {
//     ofstream outfile;      //终端输入--》内存--》文本
//     outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
//     if(!outfile) cout<<"error"<<endl;         

//     outfile<<"P0: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
//     <<0<<" "<<0<<" "<<0<<" "<<0<<" "
//      <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
//     outfile<<"P1: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
//     <<0<<" "<<0<<" "<<0<<" "<<0<<" "
//      <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
//     outfile<<"P2: "<<319.9988245765257<<" "<<0<<" "<<320.5<<" "<<0<<" "
//     <<0<<" "<<319.9988245765257<<" "<<240.5<<" "<<0<<" "
//      <<0<<" "<<0<<" "<<1<<" "<<0<<std::endl;
//     outfile<<"P3: "<<319.9988245765257<<" "<<0<<" "<<320.5<<" "<<0<<" "
//     <<0<<" "<<319.9988245765257<<" "<<240.5<<" "<<0<<" "
//      <<0<<" "<<0<<" "<<1<<" "<<0<<std::endl;
//     outfile<<"R0_rect: "<<1<<" "<<0<<" "<<0<<" "
//     <<0<<" "<<1<<" "<<0<<" "
//      <<0<<" "<<0<<" "<<1<<std::endl;
//     outfile<<"Tr_velo_to_cam: "<<0<<" "<<-1<<" "<<0<<" "<<0.25<<" "
//     <<0<<" "<<0<<" "<<-1<<" "<<-0.133<<" "
//      <<1<<" "<<0<<" "<<0<<" "<<-0.23<<std::endl;
//     outfile<<"Tr_imu_to_velo: "<<1<<" "<<0<<" "<<0<<" "<<0.23<<" "
//     <<0<<" "<<1<<" "<<0<<" "<<0<<" "
//      <<0<<" "<<0<<" "<<1<<" "<<-0.133<<std::endl;

//     outfile.close();
// }

// vector<int> project2image2dbbox(double h, double w, double l, double x, double y, double z, double yaw){
//     MatrixXd xyz(3,8);
//     xyz<<l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.,
//             0,0,0,0,-h,-h,-h,-h,
//             w / 2., -w / 2., -w / 2., w / 2., w / 2., -w / 2., -w / 2., w / 2.;
// //    cout<<"hwl"<<h<<" "<<w<<" "<<l<<endl;
// //    xyz<<-w/2, -w/2,w/2,w/2,-w/2, -w/2,w/2,w/2,
// //            -h,0,0,-h,-h,0,0,-h,
// //            l/2,l/2,l/2,l/2,-l/2,-l/2,-l/2,-l/2;

//     MatrixXd rotM (3,3);
//     rotM<<cos(yaw), 0,sin(yaw),
//             0, 1,0,
//             -sin(yaw), 0, cos(yaw);
// //    rotM=rotM.transpose();
//     xyz = rotM*xyz;
//     xyz(0,0) = xyz(0,0) + x;xyz(0,1) = xyz(0,1)  + x; xyz(0,2) = xyz(0,2) + x; xyz(0,3) = xyz(0,3) + x;
//     xyz(0,4) = xyz(0,4) + x;xyz(0,5) = xyz(0,5) + x;xyz(0,6) = xyz(0,6) + x;xyz(0,7) = xyz(0,7) + x;
//     xyz(1,0) = xyz(1,0) + y; xyz(1,1) = xyz(1,1) + y; xyz(1,2) = xyz(1,2) + y; xyz(1,3) = xyz(1,3) + y;
//     xyz(1,4) = xyz(1,4) + y;xyz(1,5) = xyz(1,5) + y;xyz(1,6) = xyz(1,6) + y;xyz(1,7) = xyz(1,7) + y;
//     xyz(2,0) = xyz(2,0) + z; xyz(2,1) = xyz(2,1) + z; xyz(2,2) = xyz(2,2) + z; xyz(2,3) = xyz(2,3) + z;
//     xyz(2,4) = xyz(2,4) + z;xyz(2,5) = xyz(2,5) + z;xyz(2,6) = xyz(2,6) + z;xyz(2,7) = xyz(2,7) + z;


//     MatrixXd P2 (3,4);
//     P2<<319.9988245765257, 0,320.5,0,
//             0,319.9988245765257,240.5,0,
//             0,0,1,0;

//     MatrixXd xyz_new_M (4,8);
//     xyz_new_M<<xyz(0,0), xyz(0,1),xyz(0,2),xyz(0,3),xyz(0,4),xyz(0,5),xyz(0,6),xyz(0,7),
//             xyz(1,0), xyz(1,1),xyz(1,2),xyz(1,3),xyz(1,4),xyz(1,5),xyz(1,6),xyz(1,7),
//             xyz(2,0), xyz(2,1),xyz(2,2),xyz(2,3),xyz(2,4),xyz(2,5),xyz(2,6),xyz(2,7),
//             1,1,1,1,1,1,1,1;
// //    cout<<"my debug bbox"<<xyz_new_M<<endl;
//     xyz=P2*xyz_new_M;

//     vector<double> x_image,y_image;
//     for(int i=0; i<8; i++){
//         x_image.push_back(xyz(0,i)/xyz(2,i));
//         y_image.push_back(xyz(1,i)/xyz(2,i));
// //        cout<<"x:"<<xyz(0,i)/xyz(2,i)<<endl;
// //        cout<<"y:"<<xyz(1,i)/xyz(2,i)<<endl;
//     }


//     vector<int> bbox;
//     bbox.push_back(int(*min_element(x_image.begin(),x_image.end())));
//     bbox.push_back(int(*min_element(y_image.begin(),y_image.end())));
//     bbox.push_back(int(*max_element(x_image.begin(),x_image.end())));
//     bbox.push_back(int(*max_element(y_image.begin(),y_image.end())));
//     if(bbox[0]<0)
//         bbox[0]=0;
//     if(bbox[1]<0)
//         bbox[1]=0;
//     if(bbox[2]>640)
//         bbox[2]=640;
//     if(bbox[3]>480)
//         bbox[3]=480;
// //    cout<<"my debug bbox"<<bbox[0]<<" "<<bbox[1]<<" "<<bbox[2]<<" "<<bbox[3]<<endl;
//     return bbox;
// }

// void saveLabel(string& out_file, vector<vector<double>> label)
// {
//     ofstream outfile;      //终端输入--》内存--》文本
//     outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
//     if(!outfile) cout<<"error"<<endl;         

//     int object_num=label.size();
//     vector<int> bbox_image;
//     for(int i=0; i<object_num; i++){

//         bbox_image=project2image2dbbox(label[i][0],label[i][1],label[i][2],label[i][3],label[i][4],label[i][5],label[i][6]);

//         outfile<<"Car "<<0<<" "<<0<<" "<<0<<" "
//         <<bbox_image[0]<<" "<<bbox_image[1]<<" "<<bbox_image[2]<<" "<<bbox_image[3]<<" "  //2dbbbox
//         <<label[i][0]<<" "<<label[i][1]<<" "<<label[i][2]<<" " //hwl
//         <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5]<<" " //xyz
//         <<label[i][6]<<" "<<0<<std::endl;  //yaw
//     }
//     outfile.close();
// }

void saveTrajectory(string& out_file, double x,double y,double z,double ow,double ox,double oy,double oz)
{
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"error"<<endl;
    outfile<<x<<" "<<y<<" "<<z<<" "    //xyz
           <<ow<<" "<<ox<<" "<<oy<<" "<<oz<<std::endl;  //
    outfile.close();
}

void gtPoseCB(const nav_msgs::OdometryConstPtr& master_traj_gt_msg){
    ROS_INFO("cb ok");
    if(!is_get_first_frame_){
        T_mas_last_.setOrigin(tf::Vector3(master_traj_gt_msg->pose.pose.position.x, master_traj_gt_msg->pose.pose.position.y, master_traj_gt_msg->pose.pose.position.z));
        T_mas_last_.setRotation(tf::Quaternion(master_traj_gt_msg->pose.pose.orientation.x,master_traj_gt_msg->pose.pose.orientation.y,
                                        master_traj_gt_msg->pose.pose.orientation.z,master_traj_gt_msg->pose.pose.orientation.w));
        is_get_first_frame_= true;
        trajectory_name_="/home/st/ubuntu_data/test_data/test5_ws/src/mecanum_robot_simulation/trajectory_tracking/data/master.txt";
        saveTrajectory(trajectory_name_, 
                master_traj_gt_msg->pose.pose.position.x,master_traj_gt_msg->pose.pose.position.y,master_traj_gt_msg->pose.pose.position.z,
                                    master_traj_gt_msg->pose.pose.orientation.w,master_traj_gt_msg->pose.pose.orientation.x,
                                    master_traj_gt_msg->pose.pose.orientation.y,master_traj_gt_msg->pose.pose.orientation.z);
        return;
    }
    // frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
    //                                                   "%06d", frame_num_));
    T_mas_cur_.setOrigin(tf::Vector3(master_traj_gt_msg->pose.pose.position.x, master_traj_gt_msg->pose.pose.position.y, master_traj_gt_msg->pose.pose.position.z));
    T_mas_cur_.setRotation(tf::Quaternion(master_traj_gt_msg->pose.pose.orientation.x,master_traj_gt_msg->pose.pose.orientation.y,
                                     master_traj_gt_msg->pose.pose.orientation.z,master_traj_gt_msg->pose.pose.orientation.w));
    tf::Transform T_detal;
    T_detal=T_mas_last_.inverse()*T_mas_cur_;
    tf::Vector3 xyz(T_detal.getOrigin().x(),T_detal.getOrigin().y(),T_detal.getOrigin().z());
    double r,p,y;
    tf::Matrix3x3(T_detal.getRotation()).getRPY(r,p,y);
    if( xyz.length()>0.5 || abs(y)>10*M_PI/180){
        trajectory_name_="/home/st/ubuntu_data/test_data/test5_ws/src/mecanum_robot_simulation/trajectory_tracking/data/master.txt";
        saveTrajectory(trajectory_name_, 
                master_traj_gt_msg->pose.pose.position.x,master_traj_gt_msg->pose.pose.position.y,master_traj_gt_msg->pose.pose.position.z,
                                    master_traj_gt_msg->pose.pose.orientation.w,master_traj_gt_msg->pose.pose.orientation.x,
                                    master_traj_gt_msg->pose.pose.orientation.y,master_traj_gt_msg->pose.pose.orientation.z);
    }
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
//    cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("cloud_accum_convert",10,true);
//    ros::Subscriber cloud_sub=nh.subscribe("/cloud",10,cloudCB);
    ros::Subscriber sub = nh.subscribe("/robot1/ground_truth/feedback", 1, gtPoseCB);
    // message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/robot1/Ruby80", 1);
    // message_filters::Subscriber<sensor_msgs::Image> image_front_sub(nh, "/robot1/front_camera/image_raw", 1);
    // message_filters::Subscriber<nav_msgs::Odometry> master_traj_sub(nh, "/robot1/ground_truth/feedback", 1);
    // message_filters::Subscriber<nav_msgs::Odometry> black_traj_sub(nh, "/husky_black/ground_truth/feedback", 1);
    // message_filters::Subscriber<nav_msgs::Odometry> green_traj_sub(nh, "/husky_green/ground_truth/feedback", 1);
    // message_filters::Subscriber<nav_msgs::Odometry> white_traj_sub(nh, "/husky_white/ground_truth/feedback", 1);
//    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_left_sub, image_right_sub,pc_sub, 10);
//    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3));
    // typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::Image, nav_msgs::Odometry,nav_msgs::Odometry,nav_msgs::Odometry,nav_msgs::Odometry> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    // message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), pc_sub, image_front_sub, master_traj_sub,black_traj_sub, green_traj_sub, white_traj_sub);
    // sync.setMaxIntervalDuration(ros::Duration(0.5));
    // ROS_INFO("ok1");
    // sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3,_4,_5,_6));
    // ROS_INFO("ok2");
    ros::spin();

    return 0;
}
